#include "ref.h"

void ref_fir_lattice_f32(const riscv_fir_lattice_instance_f32 *S, float32_t *pSrc,
                         float32_t *pDst, uint32_t blockSize)
{
    float32_t *pState;                     /* State pointer */
    const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
    float32_t *px;                         /* temporary state pointer */
    const float32_t *pk;                   /* temporary coefficient pointer */
    float32_t fcurr, fnext, gcurr, gnext;  /* temporary variables */
    uint32_t numStages = S->numStages;     /* Length of the filter */
    uint32_t blkCnt, stageCnt;             /* temporary variables for counts */

    pState = &S->pState[0];

    blkCnt = blockSize;

    while (blkCnt > 0U) {
        /* f0(n) = x(n) */
        fcurr = *pSrc++;

        /* Initialize coeff pointer */
        pk = pCoeffs;

        /* Initialize state pointer */
        px = pState;

        /* read g0(n-1) from state buffer */
        gcurr = *px;

        /* for sample 1 processing */
        /* f1(n) = f0(n) +  K1 * g0(n-1) */
        fnext = fcurr + ((*pk) * gcurr);
        /* g1(n) = f0(n) * K1  +  g0(n-1) */
        gnext = (fcurr * (*pk++)) + gcurr;

        /* save f0(n) in state buffer */
        *px++ = fcurr;

        /* f1(n) is saved in fcurr
           for next stage processing */
        fcurr = fnext;

        stageCnt = (numStages - 1U);

        /* stage loop */
        while (stageCnt > 0U) {
            /* read g2(n) from state buffer */
            gcurr = *px;

            /* save g1(n) in state buffer */
            *px++ = gnext;

            /* Sample processing for K2, K3.... */
            /* f2(n) = f1(n) +  K2 * g1(n-1) */
            fnext = fcurr + ((*pk) * gcurr);
            /* g2(n) = f1(n) * K2  +  g1(n-1) */
            gnext = (fcurr * (*pk++)) + gcurr;

            /* f1(n) is saved in fcurr1
               for next stage processing */
            fcurr = fnext;

            stageCnt--;
        }

        /* y(n) = fN(n) */
        *pDst++ = fcurr;

        blkCnt--;
    }
}

void ref_fir_lattice_q31(const riscv_fir_lattice_instance_q31 *S, q31_t *pSrc,
                         q31_t *pDst, uint32_t blockSize)
{
    q31_t *pState;                     /* State pointer */
    const q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
    q31_t *px;                         /* temporary state pointer */
    const q31_t *pk;                   /* temporary coefficient pointer */
    q31_t fcurr, fnext, gcurr, gnext;  /* temporary variables */
    uint32_t numStages = S->numStages; /* Length of the filter */
    uint32_t blkCnt, stageCnt;         /* temporary variables for counts */

    pState = &S->pState[0];

    blkCnt = blockSize;

    while (blkCnt > 0U) {
        /* f0(n) = x(n) */
        fcurr = *pSrc++;

        /* Initialize coeff pointer */
        pk = pCoeffs;

        /* Initialize state pointer */
        px = pState;

        /* read g0(n-1) from state buffer */
        gcurr = *px;

        /* for sample 1 processing */
        /* f1(n) = f0(n) +  K1 * g0(n-1) */
        fnext = (q31_t)(((q63_t)gcurr * (*pk)) >> 31) + fcurr;
        /* g1(n) = f0(n) * K1  +  g0(n-1) */
        gnext = (q31_t)(((q63_t)fcurr * (*pk++)) >> 31) + gcurr;
        /* save g1(n) in state buffer */
        *px++ = fcurr;

        /* f1(n) is saved in fcurr1
           for next stage processing */
        fcurr = fnext;

        stageCnt = (numStages - 1U);

        /* stage loop */
        while (stageCnt > 0U) {
            /* read g2(n) from state buffer */
            gcurr = *px;

            /* save g1(n) in state buffer */
            *px++ = gnext;

            /* Sample processing for K2, K3.... */
            /* f2(n) = f1(n) +  K2 * g1(n-1) */
            fnext = (q31_t)(((q63_t)gcurr * (*pk)) >> 31) + fcurr;
            /* g2(n) = f1(n) * K2  +  g1(n-1) */
            gnext = (q31_t)(((q63_t)fcurr * (*pk++)) >> 31) + gcurr;

            /* f1(n) is saved in fcurr1
               for next stage processing */
            fcurr = fnext;

            stageCnt--;
        }

        /* y(n) = fN(n) */
        *pDst++ = fcurr;

        blkCnt--;
    }
}

void ref_fir_lattice_q15(const riscv_fir_lattice_instance_q15 *S, q15_t *pSrc,
                         q15_t *pDst, uint32_t blockSize)
{
    q15_t *pState;                      /* State pointer */
    const q15_t *pCoeffs = S->pCoeffs;  /* Coefficient pointer */
    q15_t *px;                          /* temporary state pointer */
    const q15_t *pk;                    /* temporary coefficient pointer */
    q31_t fcurnt, fnext, gcurnt, gnext; /* temporary variables */
    uint32_t numStages = S->numStages;  /* Length of the filter */
    uint32_t blkCnt, stageCnt;          /* temporary variables for counts */

    pState = &S->pState[0];

    blkCnt = blockSize;

    while (blkCnt > 0U) {
        /* f0(n) = x(n) */
        fcurnt = *pSrc++;

        /* Initialize coeff pointer */
        pk = (pCoeffs);

        /* Initialize state pointer */
        px = pState;

        /* read g0(n-1) from state buffer */
        gcurnt = *px;

        /* for sample 1 processing */
        /* f1(n) = f0(n) +  K1 * g0(n-1) */
        fnext = ((gcurnt * (*pk)) >> 15U) + fcurnt;
        fnext = ref_sat_q15(fnext);

        /* g1(n) = f0(n) * K1  +  g0(n-1) */
        gnext = ((fcurnt * (*pk++)) >> 15U) + gcurnt;
        gnext = ref_sat_q15(gnext);

        /* save f0(n) in state buffer */
        *px++ = (q15_t)fcurnt;

        /* f1(n) is saved in fcurnt
           for next stage processing */
        fcurnt = fnext;

        stageCnt = (numStages - 1U);

        /* stage loop */
        while (stageCnt > 0U) {
            /* read g1(n-1) from state buffer */
            gcurnt = *px;

            /* save g0(n-1) in state buffer */
            *px++ = (q15_t)gnext;

            /* Sample processing for K2, K3.... */
            /* f2(n) = f1(n) +  K2 * g1(n-1) */
            fnext = ((gcurnt * (*pk)) >> 15U) + fcurnt;
            fnext = ref_sat_q15(fnext);

            /* g2(n) = f1(n) * K2  +  g1(n-1) */
            gnext = ((fcurnt * (*pk++)) >> 15U) + gcurnt;
            gnext = ref_sat_q15(gnext);

            /* f1(n) is saved in fcurnt
               for next stage processing */
            fcurnt = fnext;

            stageCnt--;
        }

        /* y(n) = fN(n) */
        *pDst++ = ref_sat_q15(fcurnt);

        blkCnt--;
    }
}
